#ifndef FINDERCAKE_HPP
#define FINDERCAKE_HPP

#include "deskA.hpp"
#include "Knife.hpp"
#include "FinderBase.hpp"

class FinderCake : public FinderBase
{
public:

	DeskA desk;
	Knife knife;
	
	FinderCake()
		: FinderBase()
	{
		
	}

	virtual bool ComputePath(std::vector<cv::Vec6f> &path)
	{
#if 0
		path.resize(1);
		cv::Vec2f pos = desk.Recover(cv::Vec2f(pt.x, pt.y));
		knife.GetArmPos(path[0], cv::Vec3f(pos[0], pos[1], 0.02));
		return true;
#endif

#if 1
		std::vector<cv::Vec2f> platew;
		desk.Locate(platew, frame);

		if (platew.empty())
		{
			std::cout << "nothing found\n";
			return false;
		}

		//knife.CutCake(platew.data(), path);

		//knife.CutCakeA(platew.data(), path);

		//knife.CutCakeB(platew.data(), path);

		//knife.CutCakeC(platew.data(), path);

		//knife.CutCakeD(platew.data(), path);

		//knife.CutCakeE(platew.data(), path);

		knife.CutCakeG(platew.data(), path);

		path.push_back(idlepos);

		return true;
#endif
	}




	bool ComputeH(cv::Mat &H, cv::Size patternsize = cv::Size(6, 9))
	{
		std::vector<cv::Point2f> corners; //this will be filled by the detected corners
										  //CALIB_CB_FAST_CHECK saves a lot of time on images
										  //that do not contain any chessboard corners
		if (!FindCorners(corners, frame, patternsize))
			return false;

		std::vector<cv::Point2f> corners4(4);
		corners4[1] = corners[0];
		corners4[0] = corners[patternsize.width - 1];
		corners4[2] = corners[patternsize.area()- patternsize.width];
		corners4[3] = corners.back();


		std::vector<cv::Point2f> cornersW(4);

		cv::Vec3f xyz[4] = { cv::Vec3f(0.47300,0.07568,0.25624), 
			cv::Vec3f(0.26636,0.07227,0.25154), 
			cv::Vec3f(0.27107,-0.25722,0.25105),
			cv::Vec3f(0.42841,-0.18453,0.25565) };

		cv::Vec3f rxyz[4] = { cv::Vec3f(0.2326,-2.9558,0.2848),
			cv::Vec3f(0.2326,-2.9558,0.2848),
			cv::Vec3f(0.2326,-2.9558,0.2848),
			cv::Vec3f(0.7816,3.2560,-0.2211) };

		for (int i = 0;i < 4;i++)
		{
			cv::Vec3f tmp = knife.ToBase(xyz[i], rxyz[i]);
			cornersW[i].x = tmp[0];
			cornersW[i].y = tmp[1];
		}
	


		H = cv::findHomography(corners4, cornersW);
		

		std::cout << "H=" << H << '\n';

		cv::waitKey();

		return !H.empty();
	}


	void ReCalib(int index = 0)
	{
		cv::VideoCapture c(index);
		if (c.isOpened())
		{
			c.set(CV_CAP_PROP_FRAME_WIDTH, 640);
			c.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
			c.set(CV_CAP_PROP_SETTINGS, 1);

			if (c.read(frame))
			{

				cv::imwrite("cb2.png", frame);
				cv::Mat H;
				if (ComputeH(H))
				{
					desk.Save(H, "H2.yml");
					cv::waitKey();
				}
			}
		}
	}



};


#endif
